/*
@ link : http://wit-motion.cn

@ Function:
1. Power on automatic detection sensor
2. Read acceleration, angular velocity, angle and magnetic field data
3. Set switching baud rate parameters

USB-TTL                   STM32Core              		JY901s
VCC          -----           VCC        ----        	 VCC
TX           -----           RX1  (GPIOA_10)   
RX           -----           TX1  (GPIOA_9)
GND          -----           GND    ----       			 GND
                             RX2  (GPIOA_3)  ----        TX
							 TX2  (GPIOA_2)  ----        RX
------------------------------------
*/

#include <string.h>
#include <stdio.h>
#include <math.h>
#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "misc.h"
#include "wit_c_sdk.h"
#include "UART1.h"
#include "UART2.h"
#include "delay.h"
#include "fitSphere.h"
#include "oled.h"

#define ACC_UPDATE		0x01
#define GYRO_UPDATE		0x02
#define ANGLE_UPDATE	0x04
#define MAG_UPDATE		0x08
#define READ_UPDATE		0x80
#define DIV_PI        0.31831
#define CampassX 96
#define CampassY 32

static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
const uint32_t c_uiBaud[10] = {0, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600};
static void CmdProcess(void);
static void AutoScanSensor(void);
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);
void CalcMeg(double x,double y,double z);
void CalcAcc(double x,double y,double z);
int xCursor,yCursor;
int gx,gy;
int pitch,yaw;

/* 
 * 此代码工作架构简析：
 * USART1负责输出数据
 * USART2负责与传感器通信。当接收到传感器的信息时，串口中断会接收数据并保存至内存，
                接收到结束符0x55时，会解析收到的数据，会回调之前注册的SensorDataUpdata这个函数，这个函数会根据传入的参数得知收到了哪些数据，并置XX_UPDATE标志位
从而位于main中的loop不断读取标志位从而打印更新后的数据到Uart1，并更新屏幕上的显示内容
*/

int main(void)
{
	float fAcc[3], fGyro[3], fAngle[3],fMeg[3];
	int i;
	
	SysTick_Init();
	Usart1Init(9600);
	Usart2Init(9600);
	OLED_Init(); //初始化屏幕
	OLED_DrawCircle(CampassX,CampassY,32);
	OLED_Refresh();
	WitInit(WIT_PROTOCOL_NORMAL, 0x50); //初始化传感器接口
	WitSerialWriteRegister(SensorUartSend);
	WitRegisterCallBack(SensorDataUpdata);
	WitDelayMsRegister(Delayms);
	printf("\r\n********************** wit-motion normal example  ************************\r\n");
	AutoScanSensor(); //自动搜索传感器
	while (1)
	{
		CmdProcess(); //尝试处理来自USART1的串口调试指令
		if(s_cDataUpdate) //如果传感器有新的数据送到
		{
			 for(i = 0; i < 3; i++) //解析传感器数据
			 {
					fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
					fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
					fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
					fMeg[i] = sReg[HX+i] ;
			 }
			 if(s_cDataUpdate & ACC_UPDATE) //加速度数据更新，则更新水平仪的小圆圈位置
			 {
					//printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
					if(gx!=0 && gy!=0)
						OLED_ClearCircle(gx,gy,6);
					CalcAcc(fAcc[0], fAcc[1], fAcc[2]); //将直角坐标系的加速度数据计算为球坐标系，结果保存至全局变量
					OLED_DrawCircle(gx,gy,6);
					OLED_DrawCircle(CampassX,CampassY,32);
					OLED_ShowString(0,32,"Pitch:",12);
					OLED_ShowNum(6*6,32,pitch,3,12);	  
					s_cDataUpdate &= ~ACC_UPDATE;
			 }
			 if(s_cDataUpdate & GYRO_UPDATE)
			 {
					//printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
					s_cDataUpdate &= ~GYRO_UPDATE;
			 }
			 if(s_cDataUpdate & ANGLE_UPDATE)
			 {
					//printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
					s_cDataUpdate &= ~ANGLE_UPDATE;
			 }
			 if(s_cDataUpdate & MAG_UPDATE) //磁场数据更新，计算北方位置，显示在屏幕上
			 {
					Point np = {fMeg[0], fMeg[1], fMeg[2]};
					//printf("meg:%.2f %.2f %.2f\r\n", fMeg[0], fMeg[1], fMeg[2]);
					OLED_ClearLine(CampassX,CampassY,xCursor,yCursor);
					CalcMeg(fMeg[0], fMeg[1], fMeg[2]);
					OLED_DrawLine(CampassX,CampassY,xCursor,yCursor);
					OLED_ShowString(0,44,"Yaw:",12);
					OLED_ShowNum(6*6,44,yaw,3,12);
					OLED_Refresh();
					//printf("\r\n");
					s_cDataUpdate &= ~MAG_UPDATE;
			 }
		}    
	}
}

void CalcAcc(double x,double y,double z) { //将直角坐标系的加速度数据计算为球坐标系，结果保存至全局变量
	double RXY = sqrt(x*x + y*y); //XY平面投影加速度大小
	double R = sqrt(RXY*RXY + z*z); //总加速度大小
	double AngleXY = atan2(y,x); //XY屏幕加速度角度
	gx = CampassX-30*RXY/R*cos(AngleXY); //小圆在屏幕上投影的位置
  gy = CampassY+30*RXY/R*sin(AngleXY);
	double AngleZ = RXY < 0.01 ? 90 : atan2(z,RXY)*DIV_PI*180; //加速度与Z轴的夹角
	pitch = 90-AngleZ;
}
void CalcMeg(double x,double y,double z) {
//日后可能会用到的磁场校准代码
//	// Hard-Iron Parameters
//	const double hard_iron[3] = {34.369, 19.122, -9.663};
//	// Soft-Iron Parameters
//	const float soft_iron[3][3] = {
//			{1.0, 0, 0}, 
//		{0, 0.8938, 0}, 
//		{0, 0, 0.8005}
//	};
//	// Hard-Iron Calibration
//	int hi_cal[3];
//	hi_cal[0] = x - hard_iron[0];
//	hi_cal[1] = y - hard_iron[1];
//	hi_cal[2] = z - hard_iron[2];
//	// Soft-Iron Calibration
//	float mag_data[3];
//	for (int i = 0; i < 3; i++) {
//		mag_data[i] = (soft_iron[i][0] * hi_cal[0]) +
//									(soft_iron[i][1] * hi_cal[1]) +
//									(soft_iron[i][2] * hi_cal[2]);
//	}
//	x = mag_data[0]* 0.00833;
//	y = mag_data[1]* 0.00833;
//	z = mag_data[2]* 0.00833;
	double MegRXY = sqrt(x*x + y*y); //XY平面磁场大小
	double MegR = sqrt(MegRXY*MegRXY + z*z); //总磁场大小
	double MegAngleXY = atan2(y,x); //XY平面磁场相对X轴角度，弧度制
	xCursor = CampassX+30*cos(MegAngleXY); //屏幕上磁场指针指向的位置
	yCursor = CampassY-30*sin(MegAngleXY);
	double MegAngleZ = atan2(z,MegRXY)*DIV_PI*180; //磁场相对Z轴角度，角度值
	yaw = (int)(MegAngleXY*DIV_PI*180+270) % 360; //修正后显示在屏幕上的磁场角度
	printf("%.2f %.2f %.2f %.2f %d\r\n",x,y,MegRXY,MegR,yaw); //串口调试数据
	//printf("MegR: %.2lf %.2lf\r\nMegAngle: %.2lf %.2lf\r\n",MegRXY,MegR,MegAngleXY,MegAngleZ);
}
void CopeCmdData(unsigned char ucData)
{
	 static unsigned char s_ucData[50], s_ucRxCnt = 0;
	
	 s_ucData[s_ucRxCnt++] = ucData;
	 if(s_ucRxCnt<3)return;										//Less than three data returned
	 if(s_ucRxCnt >= 50) s_ucRxCnt = 0;
	 if(s_ucRxCnt >= 3)
	 {
		 if((s_ucData[1] == '\r') && (s_ucData[2] == '\n'))
		 {
		  	s_cCmd = s_ucData[0];
			  memset(s_ucData,0,50);//
			  s_ucRxCnt = 0;
	   } 
		 else 
		 {
			 s_ucData[0] = s_ucData[1];
			 s_ucData[1] = s_ucData[2];
			 s_ucRxCnt = 2;
			}
	  }
}

static void ShowHelp(void)
{
	printf("\r\n************************	 WIT_SDK_DEMO	************************");
	printf("\r\n************************          HELP           ************************\r\n");
	printf("UART SEND:a\\r\\n   Acceleration calibration.\r\n");
	printf("UART SEND:m\\r\\n   Magnetic field calibration,After calibration send:   e\\r\\n   to indicate the end\r\n");
	printf("UART SEND:U\\r\\n   Bandwidth increase.\r\n");
	printf("UART SEND:u\\r\\n   Bandwidth reduction.\r\n");
	printf("UART SEND:B\\r\\n   Baud rate increased to 115200.\r\n");
	printf("UART SEND:b\\r\\n   Baud rate reduction to 9600.\r\n");
	printf("UART SEND:R\\r\\n   The return rate increases to 10Hz.\r\n");
	printf("UART SEND:r\\r\\n   The return rate reduction to 1Hz.\r\n");
	printf("UART SEND:C\\r\\n   Basic return content: acceleration, angular velocity, angle, magnetic field.\r\n");
	printf("UART SEND:c\\r\\n   Return content: acceleration.\r\n");
	printf("UART SEND:h\\r\\n   help.\r\n");
	printf("******************************************************************************\r\n");
}

static void CmdProcess(void)
{
	switch(s_cCmd)
	{
		case 'a':	
			if(WitStartAccCali() != WIT_HAL_OK) 
				printf("\r\nSet AccCali Error\r\n");
			break;
		case 'm':	
			if(WitStartMagCali() != WIT_HAL_OK) 
				printf("\r\nSet MagCali Error\r\n");
			break;
		case 'e':	
			if(WitStopMagCali() != WIT_HAL_OK)
				printf("\r\nSet MagCali Error\r\n");
			break;
		case 'u':	
			if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) 
				printf("\r\nSet Bandwidth Error\r\n");
			break;
		case 'U':	
			if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) 
				printf("\r\nSet Bandwidth Error\r\n");
			break;
		case 'B':	
			if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) 
				printf("\r\nSet Baud Error\r\n");
			else 
				Usart2Init(c_uiBaud[WIT_BAUD_115200]);											
			break;
		case 'b':	
			if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
				printf("\r\nSet Baud Error\r\n");
			else 
				Usart2Init(c_uiBaud[WIT_BAUD_9600]);												
			break;
		case 'R':	
			if(WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK) 
				printf("\r\nSet Rate Error\r\n");
			break;
		case 'r':	
			if(WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) 
				printf("\r\nSet Rate Error\r\n");
			break;
		case 'C':	
			if(WitSetContent(RSW_ACC|RSW_GYRO|RSW_ANGLE|RSW_MAG) != WIT_HAL_OK) 
				printf("\r\nSet RSW Error\r\n");
			break;
		case 'c':	
			if(WitSetContent(RSW_ACC) != WIT_HAL_OK) 
				printf("\r\nSet RSW Error\r\n");
			break;
		case 'h':
			ShowHelp();
			break;
	}
	s_cCmd = 0xff;
}

static void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
	Uart2Send(p_data, uiSize);
}

static void Delayms(uint16_t ucMs)
{
	delay_ms(ucMs);
}

static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
	int i;
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
//            case AX:
//            case AY:
            case AZ:
				s_cDataUpdate |= ACC_UPDATE;
            break;
//            case GX:
//            case GY:
            case GZ:
				s_cDataUpdate |= GYRO_UPDATE;
            break;
//            case HX:
//            case HY:
            case HZ:
				s_cDataUpdate |= MAG_UPDATE;
            break;
//            case Roll:
//            case Pitch:
            case Yaw:
				s_cDataUpdate |= ANGLE_UPDATE;
            break;
            default:
				s_cDataUpdate |= READ_UPDATE;
			break;
        }
		uiReg++;
    }
}

static void AutoScanSensor(void)
{
	int i, iRetry;
	
	for(i = 1; i < 10; i++)
	{
		Usart2Init(c_uiBaud[i]);
		iRetry = 2;
		do
		{
			s_cDataUpdate = 0;
			WitReadReg(AX, 3);
			delay_ms(100);
			if(s_cDataUpdate != 0)
			{
				printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
				ShowHelp();
				return ;
			}
			iRetry--;
		}while(iRetry);		
	}
	printf("can not find sensor\r\n");
	printf("please check your connection\r\n");
}

//第一版的磁场校准代码，已弃用
//#define CaliDistance 20
//int CaliPointCount = 0;
//Point MegCaliPoints[4];
//Point MegOffset;
//void MegCalibration(Point newPoint) {
//	int i;
//	for(i=0;i<CaliPointCount;i++) {
//		if(!FartherThan(newPoint,MegCaliPoints[i],CaliDistance*CaliDistance)) //如果数据点距离太近，不作为用来校准的点
//			return;
//	}
//	if(CaliPointCount < 4) { //数据点合适，作为用于校准的点
//		MegCaliPoints[CaliPointCount] = newPoint;
//		CaliPointCount++;
//	} else { //集齐4个用于校准的点
//		if(fitSphere(MegCaliPoints[0],MegCaliPoints[1],MegCaliPoints[2],MegCaliPoints[3],&MegOffset)) {
//			printf("Auto Meg Calibration done.\r\n");
//		}
//		CaliPointCount = 0;
//	}
//}
